#include "AStar.h"
#include "../occupancy_map.h"

AStar::AStar()
{
	startPathPoint = NULL;
	goalPathPoint = NULL;
	g_score = NULL;
	h_score = NULL;
	closedSet = NULL;
}


PathPoint* AStar::getStartPathPoint()
{
	return startPathPoint;
}


PathPoint* AStar::getGoalPathPoint()
{
	return goalPathPoint;
}


void AStar::setStartPathPoint(PathPoint *start_)
{
	delete startPathPoint;
	startPathPoint = new PathPoint (start_->getX(), start_->getY(), start_->getAngle());
}


void AStar::setGoalPathPoint(PathPoint *goal_)
{
	delete goalPathPoint;
	goalPathPoint = new PathPoint (goal_->getX(), goal_->getY(), goal_->getAngle());
}


void AStar::launchAstar(const Coordinates &start,
		const Coordinates &goal,
		const OccupancyMap &map, Trajectory *traj)
{
	int i, j, w;
	PathPoint* path;
	Coordinates start_grid = start;
	Coordinates goal_grid = goal;
	map.euclideToGrid(&start_grid);
	map.euclideToGrid(&goal_grid);


	startPathPoint = new PathPoint (start_grid.first, start_grid.second, 0);
	goalPathPoint = new PathPoint (goal_grid.first, goal_grid.second, 0);
	dim_dem = map.getSize();
	ROS_INFO("Computing path from %d,%d to %d,%d", startPathPoint->getX(),
			      startPathPoint->getY(), goalPathPoint->getX(), goalPathPoint->getY());

	g_score = new long**[dim_dem];
	h_score = new long**[dim_dem];
	closedSet = new bool*[dim_dem];


	for (i=0; i<dim_dem; i++)
	{
		g_score[i] = new long*[dim_dem];
		h_score[i] = new long*[dim_dem];
		closedSet[i] = new bool[dim_dem];
		for (j=0; j<dim_dem; j++)
		{
			g_score[i][j] = NULL;
			h_score[i][j] = NULL;
			g_score[i][j] = new long[POSSIBLE_DIR];
			h_score[i][j] = new long[POSSIBLE_DIR];
			closedSet[i][j] = false;
			for (w=0; w<POSSIBLE_DIR; w++)
			{
				g_score[i][j][w] = -1;
				h_score[i][j][w] = -1;
			}
		}
	}

	path = NULL;
	path = compute(map);

	while (path != NULL) {
		Coordinates coordinate(path->getX(), path->getY());
		map.gridToEuclide(&coordinate);
    traj->push_front(coordinate);

		PathPoint *old_point = path;
		path = path->getCameFrom();
		delete old_point;
	}
}


PathPoint* AStar::compute(const OccupancyMap &map)
{

	list <PathPoint*> neighbor;
	list <PathPoint*>::iterator it, it2;
	int i, j, w;
	int pos1X, pos1Y, pos1Orient, pos2X, pos2Y, pos2Orient;
	int goalX, goalY, goalOrient;

	ros::Time begin_time = ros::Time::now();

	PathPoint *pos1, *pos2, *posTmp;
	PathPoint *path = NULL;
	bool tentativeIsBetter;
	long tentativeGScore, pos2FScore, distBetween;
	bool find;
	int nodiAnalizzati = 0;
	bool goal_cell = false;

	for (i=0; i<dim_dem; i++)
	{
		for (j=0; j<dim_dem; j++)
		{
			closedSet[i][j] = false;
			for (w=0; w<POSSIBLE_DIR; w++)
			{
				g_score[i][j][w] = -1;
				h_score[i][j][w] = -1;
			}
		}
	}
	deleteAllPathPoint();

	openSet.push_back( new PathPoint(startPathPoint->getX(), startPathPoint->getY(), startPathPoint->getAngle()) );
	g_score [startPathPoint->getX()][startPathPoint->getY()][startPathPoint->getAngle()] = 0;
	h_score [startPathPoint->getX()][startPathPoint->getY()][startPathPoint->getAngle()] = heuristicEstimateOfDistance(startPathPoint, goalPathPoint);

	goalX = goalPathPoint->getX();
	goalY = goalPathPoint->getY();
	goalOrient = goalPathPoint->getAngle();

	while (openSet.empty() == false)
	{
		if (ros::Time::now() - begin_time > ros::Duration(10)) {
			ROS_ERROR("AStar is taking too long to compute. Terminated");
			deleteAllPathPoint();
			for (it = neighbor.begin(); it != neighbor.end(); ++it)
				delete *it;
      return NULL;
		}

		it = openSet.end();
		it--;
		pos1 = *it;
		pos1X = pos1->getX();
		pos1Y = pos1->getY();
		pos1Orient = pos1->getAngle();
		if ((pos1X == goalX) && (pos1Y == goalY)){
			path = copyPath(pos1);
			return path;
		}
		closedSet [pos1X][pos1Y] = true;
		openSet.pop_back();
		visitedPos.push_back(pos1);
		for (it=neighbor.begin(); it != neighbor.end(); ++it) {
			delete *it;
		}
		neighbor.clear();

		findNeighborPathPoints(pos1, neighbor);

		for ( it=neighbor.begin() ; it != neighbor.end(); ++it ){
			pos2 = *it;
			pos2X = pos2->getX();
			pos2Y = pos2->getY();
			pos2Orient = pos2->getAngle();
			nodiAnalizzati++;

			if((pos1X == goalX) && (pos1Y == goalY))
				goal_cell = true;

			distBetween = calc.distBetween(pos1, pos2, map, goal_cell);
			if (distBetween == MIN_LONG)
			{
				delete pos2;
				*it = NULL;
			}
			else
			{
				tentativeGScore = g_score[pos1X][pos1Y][pos1Orient] + distBetween;

				tentativeIsBetter = false;
				if (g_score [pos2X][pos2Y][pos2Orient] == -1)
					tentativeIsBetter = true;
				else
					if (tentativeGScore < g_score[pos2X][pos2Y][pos2Orient])
					{
						tentativeIsBetter = true;
						for ( it2=openSet.begin() ; it2!=openSet.end(); it2++ ){
							posTmp = *it2;
							if (posTmp == pos2) {
								openSet.erase(it2);
								delete posTmp;
								break;
							}
						}
					}

				if (tentativeIsBetter == true)
				{
					g_score[pos2X][pos2Y][pos2Orient] = tentativeGScore;
					h_score[pos2X][pos2Y][pos2Orient] = heuristicEstimateOfDistance(pos2, goalPathPoint);
					pos2FScore = g_score[pos2X][pos2Y][pos2Orient] + h_score[pos2X][pos2Y][pos2Orient];

					pos2->setCameFrom(pos1);
					pos2->setFScore(pos2FScore);

					find = false;
					it2 = openSet.end();
					while ((it2 != openSet.begin()) && (find == false))
					{
						it2--;
						posTmp = *it2;
						if (posTmp->getFScore() > pos2FScore)
						{
							it2++;
							openSet.insert (it2,pos2);
							*it = NULL;
							find = true;
						}
					}
					if (find == false)
						openSet.push_front(pos2);
					*it = NULL;
				}
			}
		}
	}
	return NULL;	//Failure
}


PathPoint* AStar::copyPath(PathPoint *pos)
{
	PathPoint *tmp1, *tmp2, *path;
	PathPoint *previousPos;

	path = new PathPoint(pos->getX(), pos->getY(), pos->getAngle(), pos->getFScore(), NULL);
	tmp1 = path;

	previousPos = pos;
	while (previousPos->getCameFrom() != NULL)
	{
		previousPos = previousPos->getCameFrom();
		tmp2 = new PathPoint(previousPos->getX(), previousPos->getY(), previousPos->getAngle(), previousPos->getFScore(), NULL);
		tmp1->setCameFrom(tmp2);
		tmp1 = tmp2;
	}

	return path;
}


long AStar::heuristicEstimateOfDistance(PathPoint *pos1, PathPoint *pos2)
{
	long distX, distY, val;

	distX = fabs(pos2->getX()-pos1->getX());
	distY = fabs(pos2->getY()-pos1->getY());

	if (distX < distY)
		val = WEIGHT_FLAT_DIAG*distX + (distY-distX)*WEIGHT_FLAT;
	else
		val = WEIGHT_FLAT_DIAG*distY + (distX-distY)*WEIGHT_FLAT;

	return val;
}


void AStar::findNeighborPathPoints(PathPoint *pos, list<PathPoint*>& neighbor)
{
	int x, y, orient, posX, posY;
	PathPoint *newPos;

	posX = pos->getX();
	posY = pos->getY();
	x = posX + 1;
	y = posY;
	orient = 2;
	if ( (checkBoundaries(x, y) == true) && (closedSet[x][y] == false) )
	{
		newPos = new PathPoint(x,y,orient);
		neighbor.push_back(newPos);
	}
	x = posX - 1;
	y = posY;
	orient = 6;
	if ( (checkBoundaries(x, y) == true) && (closedSet[x][y] == false) )
	{
		newPos = new PathPoint(x,y,orient);
		neighbor.push_back(newPos);
	}
	x = posX;
	y = posY + 1;
	orient = 0;
	if ( (checkBoundaries(x, y) == true) && (closedSet[x][y] == false) )
	{
		newPos = new PathPoint(x,y,orient);
		neighbor.push_back(newPos);
	}
	x = posX;
	y = posY - 1;
	orient = 4;
	if ( (checkBoundaries(x, y) == true) && (closedSet[x][y] == false) )
	{
		newPos = new PathPoint(x,y,orient);
		neighbor.push_back(newPos);
	}
	x = posX + 1;
	y = posY + 1;
	orient = 1;
	if ( (checkBoundaries(x, y) == true) && (closedSet[x][y] == false) )
	{
		newPos = new PathPoint(x,y,orient);
		neighbor.push_back(newPos);
	}
	x = posX + 1;
	y = posY - 1;
	orient = 3;
	if ( (checkBoundaries(x, y) == true) && (closedSet[x][y] == false) )
	{
		newPos = new PathPoint(x,y,orient);
		neighbor.push_back(newPos);
	}
	x = posX - 1;
	y = posY - 1;
	orient = 5;
	if ( (checkBoundaries(x, y) == true) && (closedSet[x][y] == false) )
	{
		newPos = new PathPoint(x,y,orient);
		neighbor.push_back(newPos);
	}
	x = posX - 1;
	y = posY + 1;
	orient = 7;
	if ( (checkBoundaries(x, y) == true) && (closedSet[x][y] == false) )
	{
		newPos = new PathPoint(x,y,orient);
		neighbor.push_back(newPos);
	}

}


bool AStar::checkBoundaries(int x, int y)
{
	if ((x < (0 + DIM_ROVER + 1)) || (x > (dim_dem - 1 - DIM_ROVER - 1)) || (y < (0 + DIM_ROVER + 1)) || (y > (dim_dem - 1 - DIM_ROVER - 1)))
		return false;
	else
		return true;
}


void AStar::deleteAllPathPoint()
{
	list <PathPoint*>::iterator it;

	for ( it=openSet.begin() ; it != openSet.end(); it++ )
		delete *it;

	for ( it=visitedPos.begin() ; it != visitedPos.end(); it++ )
		delete *it;

	openSet.clear();
	visitedPos.clear();
}


AStar::~AStar(void)
{
	int i, j;


	delete startPathPoint;
	delete goalPathPoint;

	deleteAllPathPoint();

	for (i=0; i<dim_dem; i++)
	{
		for (j=0; j<dim_dem; j++)
		{

			delete [] g_score[i][j];
			delete [] h_score[i][j];
		}
		delete [] g_score[i];
		delete [] h_score[i];
		delete [] closedSet[i];
	}
	delete [] g_score;
	delete [] h_score;
	delete [] closedSet;

}
